#include "adpcm.h"

// ----------- ADPCM 压缩算法 ----------------------------------------------------------------------------
/* Quantizer step size lookup table */
const uint16_t StepSizeTable[89]={7,8,9,10,11,12,13,14,16,17,
                            19,21,23,25,28,31,34,37,41,45,
                            50,55,60,66,73,80,88,97,107,118,
                            130,143,157,173,190,209,230,253,279,307,
                            337,371,408,449,494,544,598,658,724,796,
                            876,963,1060,1166,1282,1411,1552,1707,1878,2066,
                            2272,2499,2749,3024,3327,3660,4026,4428,4871,5358,
                            5894,6484,7132,7845,8630,9493,10442,11487,12635,13899,
                            15289,16818,18500,20350,22385,24623,27086,29794,32767};
                            
/* Table of gindex changes */
const int8_t IndexTable[16]={-1,-1,-1,-1,2,4,6,8,-1,-1,-1,-1,2,4,6,8};
static int8_t  gindex = 0;
static int32_t gpredsample = 0;

/*******************************************************************************
* Function Name  : ADPCM_Decode() ADPCM解码. 4bits解码为2字节
* Description    : ADPCM_Decode.
* Input          : code: a byte containing a 4-bit ADPCM sample. 
* Output         : None
* Return         : 16-bit ADPCM sample
*******************************************************************************/
void ADPCM_Init(void)
{
	gindex = 0;
	gpredsample = 0;	
}

int16_t ADPCM_Decode(uint8_t code)
{

  uint16_t step=0;
  int32_t diffq=0;
  
  step = StepSizeTable[gindex];

  /* 2. inverse code into diff */
  diffq = step>> 3;
  if (code&4)
  {
    diffq += step;
  }
  
  if (code&2)
  {
    diffq += step>>1;
  }
  
  if (code&1)
  {
    diffq += step>>2;
  }

  /* 3. add diff to predicted sample*/
  if (code&8)
  {
    gpredsample -= diffq;
  }
  else
  {
    gpredsample += diffq;
  }
  
  /* check for overflow*/
  if (gpredsample > 32767)
  {
    gpredsample = 32767;
  }
  else if (gpredsample < -32768)
  {
    gpredsample = -32768;
  }

  /* 4. find new quantizer step size */
  gindex += IndexTable [code];
  /* check for overflow*/
  if (gindex < 0)
  {
    gindex = 0;
  }
  if (gindex > 88)
  {
    gindex = 88;
  }
  
  /* 5. save predict sample and gindex for next iteration */
  /* done! static variables */
  
  /* 6. return new speech sample*/
  return ((int16_t)gpredsample);
}
